#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

class ImuDataNode : public rclcpp::Node
{
public:
    ImuDataNode() : Node("imu_data_node")
    {
        imu_sub =
            this->create_subscription<sensor_msgs::msg::Imu>("/imu/data", 10, std::bind(&ImuDataNode::IMUDataCallback, this, std::placeholders::_1));
    }

private:
    void IMUDataCallback(const sensor_msgs::msg::Imu::SharedPtr msg);
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub;
};

void ImuDataNode::IMUDataCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
    double roll, pitch, yaw;
    tf2::Quaternion tf2_quat(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
    tf2::Matrix3x3(tf2_quat).getRPY(roll, pitch, yaw);

    roll = roll * 180 / M_PI;
    pitch = pitch * 180 / M_PI;
    yaw = yaw * 180 / M_PI;

    RCLCPP_INFO(this->get_logger(), "Roll: %f, Pitch: %f, Yaw: %f", roll, pitch, yaw);
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ImuDataNode>());
    rclcpp::shutdown();
    return 0;
}